#include "outputSlave.h"
#include <RGB.h>
#include <RGBMatrix.h>
#include <MultiMatrix.h>
#include <Servo.h>
#include <String.h>
#include <SlaveData.h>
#include "StackList.h"
#include "StackArray.h"

// Set these to enable/disable components
#define USE_LED 1
#define USE_SERVOS 1

#define SHOW_BARGRAPH USE_LED & 0
#define SHOW_TIMEGRAPH USE_LED & 0
#define SHOW_AMBIENT USE_LED & 0

#define HEAD_SERVO_PIN 9
#define PETALS_SERVO_PIN 8

#define HEAD_SERVO_MAX 90
#define HEAD_SERVO_MIN 30

#define PETALS_SERVO_MAX 120
#define PETALS_SERVO_MIN 30

#define HEAD_SERVO_DELAY 25
#define PETALS_SERVO_DELAY 50

// smaller => faster, but shit may break
#define LOOP_DELAY 1500

Servo servoHeadTilt;
Servo servoPetals;

#define DEBUG 0

byte hmReceived = 0;

// Initialize hardware
void setup() {
  //Serial.println("petals 180");  
  // Matrix LEDs
#if USE_LED == 1
  MultiMatrix.begin(0, NORTH, 1, NORTH, 2, SOUTH, 3, SOUTH);
  MultiMatrix.clear();
#endif

#if USE_SERVOS == 1
  servoHeadTilt.attach(HEAD_SERVO_PIN);
  servoPetals.attach(PETALS_SERVO_PIN);
  //Close the flower initially
  servoPetals.write(180);
#endif

  Serial.begin(9600);
#if DEBUG == 1
  output("outputSlave initialized...");
#endif
  output(COMMAND_COMPLETE);

}

String input;
void loop() {

#if SHOW_BARGRAPH == 1
  barGraph(buffer, 12);
  MultiMatrix.display();
  delay(5000);
  return;
#endif

#if SHOW_TIMEGRAPH == 1
  timeGraph(buffer, 23, TIME_GRAPH_RATE);
  MultiMatrix.display();
  delay(5000);
  return;
#endif

  #if DEBUG == 2
  output("Free RAM: "+freeRam());
  #endif
  
  readCommand();
  delay(LOOP_DELAY);
  output(COMMAND_COMPLETE);
}

// I wanted to make this variable static in readMessage, but the compiler complained.
// Only readMessage should know about this var.
// Keep incoming characters in a linked list, to save difficulty with allocating char buffers
// This function will allocate memory for ppszOutMessage. The caller is responsible for freeing it.
StackList <char> stackCurrentMessage;
const unsigned int readMessage(char** ppszOutMessage) {
  
  // this only needs to happen once, but w/e.
  stackCurrentMessage.setPrinter(Serial);
  
  while (Serial.available() > 0) {
    stackCurrentMessage.push((char)Serial.read());

    if (stackCurrentMessage.peek() == MESSAGE_END) {
      const unsigned int lenMessage = stackCurrentMessage.count();
      *ppszOutMessage = (char*)malloc(sizeof(char) * lenMessage);
      
      for (int iCurrChar = lenMessage - 1; iCurrChar >= 0; iCurrChar--) {
        char currChar = stackCurrentMessage.pop();
        (*ppszOutMessage)[iCurrChar] = currChar;
      }
      
      return lenMessage;
    }
  }

  return 0;
}



void readCommand() {
  char * pszMessage;
  unsigned int lenMessage = 0;
  while ((lenMessage = readMessage(&pszMessage)) > 0) {
    
    String command = (String)pszMessage;
    //String recvd = "outputSlave received command: ";
    
    if ( command.startsWith(ACTIVATE_PETALS_AND_FLOWER) || command.startsWith(DEACTIVATE_PETALS_AND_FLOWER)) {
      command.startsWith(ACTIVATE_PETALS_AND_FLOWER) ? activatePetalsAndFlower() : deactivatePetalsAndFlower();
    }

    // This can probably be simplified
    else if ( command.startsWith(RINGS) ) {
      
      String change; 
      String goal;
      char colorToFill = BLACK;
      
      //Serial.println("rings command;");
      
      change = command.substring(command.indexOf(ARG_DELIMITER)+1, command.length());
      if (change.startsWith(INCREASING)) {
      //  Serial.println("INCREASING");
        change = INCREASING;
      }
      else if ( change.startsWith(NO_CHANGE) ) {
      //  Serial.println("NO CHANGE");
        change = NO_CHANGE;
      }
      else {
        //Serial.println("GOT DECREASING");
        change = DECREASING;
      }
      
      goal = command.substring(command.lastIndexOf(ARG_DELIMITER)+1, command.length());
      if (goal.startsWith(BELOW_GOAL)) {
        //Serial.println("below goal");
        colorToFill = UNDER_GOAL_COLOR;
      }
      else if (goal.startsWith(AT_GOAL)) {
        //Serial.println("at goal");
        colorToFill = AT_GOAL_COLOR;
      }
      else {
      //  Serial.println("over goal");
        colorToFill = OVER_GOAL_COLOR;
      }
      //output(recvd+command);
     rings(colorToFill, LINE_COLOR, change == INCREASING, (change == NO_CHANGE ? 0 : RING_RATE), 1);
    }
    
    else if ( command.startsWith(TIME_GRAPH) || command.startsWith(BAR_GRAPH) ) {
      
      int idx = command.indexOf(ARG_DELIMITER);
      String num;
      int numValues = 0;
      for (int i = idx+1; i < command.length(); i++) {
          if ( command.charAt(i) == ARG_DELIMITER || command.charAt(i) == MESSAGE_END ) {
            numValues++;
          }
      }
      
      int valueBuffer[numValues];
      int valueIndex = 0;
      for (int i = idx+1; i < command.length(); i++) {
        if ( command.charAt(i) == ARG_DELIMITER || command.charAt(i) == MESSAGE_END ) {
          char tmp[num.length()+1];
          num.toCharArray(tmp, num.length()+1);
          valueBuffer[valueIndex++] = atoi(tmp);
          num = "";
        }
        else {
          num += command.charAt(i); 
        }
      }

      if (command.startsWith(TIME_GRAPH)) timeGraph(valueBuffer, numValues, NULL);
      else barGraph(valueBuffer, numValues);     
    }
    else if (command.startsWith(HEAT_MAP)) {
      if (hmReceived == 0) MultiMatrix.clear();
      output(command);
      heatMap(hmReceived, &command[command.indexOf(ARG_DELIMITER)+1]);
      hmReceived = (hmReceived + 1) % 16;
    }
      
    else {
     output("X: "+command); 
    }
    
    // not sure this does the trick?
    free(pszMessage);
  }

}

void deactivatePetalsAndFlower() {
      MultiMatrix.clear(); 
      for (int i = HEAD_SERVO_MAX; i > HEAD_SERVO_MIN; i--) {
      servoHeadTilt.write(i);
      delay(HEAD_SERVO_DELAY);
    }
    
    for (int i = PETALS_SERVO_MIN; i < PETALS_SERVO_MAX; i++) {
      servoPetals.write(i);
      delay(PETALS_SERVO_DELAY);
    }
 
}

void activatePetalsAndFlower() {
    for (int i = HEAD_SERVO_MIN; i < HEAD_SERVO_MAX; i++) {
    servoHeadTilt.write(i);
    delay(HEAD_SERVO_DELAY);
  }
  
  for (int i = PETALS_SERVO_MAX; i > PETALS_SERVO_MIN; i--) {
    servoPetals.write(i);
    delay(PETALS_SERVO_DELAY);
  }
}

void heatMap( int row, char * buffer ) {
    for (int col = 0; col < LED_COUNT; col++) {
      byte val = buffer[col];
      val = min(val, WHITE);
      val = max(val, BLACK);
      MultiMatrix.fillPixel(col, LED_COUNT - row, val);      
    }
  MultiMatrix.display();
}

// show moving graph of an arbitrary number of datapoints 
void timeGraph ( int * buffer, int len, int rate ) {

  if ( rate == NULL ) { 
    rate = 100; 
  }

#if DEBUG == 1
  output("In timeGraph()"); 
#endif

  for (int step = 0; step < len; step++) {

#if USE_LED == 1
    MultiMatrix.clear(); 
#endif
    int i = (step >= 15 ? step - 15 : 0);
    for (int column = (step >= 15 ? 15 : step); column >= 0; column--) {

      int value = map(buffer[i], column, UPPER_GOAL_THRESHOLD, 0, 14);
      if (value > 14) { 
        value = 14; 
      }
      i++;

#if DEBUG == 2
      Serial.print(value);   
      Serial.print(" "); 
#endif

#if USE_LED == 1
      MultiMatrix.fillPixel(14-value, column, (value > 4 ? (value > 9 ? RED : ORANGE) : GREEN));
      MultiMatrix.display(); 
#endif
    }

    delay(rate);
  }
}

// Show a bar graph for 12 months of data
// buffer index 0 is the leftmost bar in the
// graph.
void barGraph ( int * buffer, int len ) {

  if (len > 12 || len < 1) { 
    return; 
  }

#if USE_LED == 1
  MultiMatrix.clear(); 

  // draw the x and y axes
  MultiMatrix.fillRow(15, TEAL);
  MultiMatrix.fillColumn(15, TEAL);
#endif
 
  int start_column = 13;
  int i = 0;
  for ( int column = start_column; column > (start_column - len); column-- ) {
    int value = map(buffer[i], 0, UPPER_GOAL_THRESHOLD, 1, 13);
#if DEBUG == 2
    Serial.print(buffer[i], DEC);
    Serial.print("-->");
    Serial.println(value, DEC);
#endif
#if USE_LED == 1
    fillBar(column, value);
#endif
    i++;
  }
#if USE_LED == 1
  MultiMatrix.display();
#endif
}

// fill specified column using color gradient based on value
void fillBar(int column, int value) {
  for ( int row = 14; row >= 0; row-- ) {
    if ( row >= 14-value ) {
      MultiMatrix.fillPixel(row, column, (row > 4 ? (row > 9 ? GREEN : ORANGE) : 
      RED));
    }
    else {
      MultiMatrix.fillPixel(row, column, BLACK);
    }
  }     
}

void rings(char bgColor, char lineColor, boolean isGrowing, int rate, int iterations) {

  if ( rate == 0 ) {
    MultiMatrix.fillScreen(bgColor); 
    MultiMatrix.display();
    delay(100);
    return; 
  }
  
  if ( isGrowing ) {
    do {
      for ( int ringNum = 7; ringNum >= 0; ringNum-- ) {

        MultiMatrix.fillScreen(bgColor);
        if ( ringNum <= 4 ) {
          fillRingNum(ringNum + 3, lineColor);
        }
        if ( ringNum <= 1 ) {
          fillRingNum(ringNum + 6, lineColor);
        }

        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }

      for ( int ringNum = 2; ringNum >= 0; ringNum-- ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum, lineColor);
        fillRingNum(ringNum + 3, lineColor);
        MultiMatrix.display();
        delay(rate);
      }

      for ( int ringNum = 2; ringNum >= 0; ringNum-- ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      MultiMatrix.fillScreen(bgColor);
      MultiMatrix.display();
    }

    while ( --iterations > 0 );    
  }
  //growing
  else {

    do {
      for ( int ringNum = 0; ringNum < 8; ringNum++ ) {

        MultiMatrix.fillScreen(bgColor);
        if ( ringNum >= 3 ) {
          fillRingNum(ringNum - 3, lineColor);
        }
        if ( ringNum >= 6 ) {
          fillRingNum(ringNum - 6, lineColor);
        }

        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      for ( int ringNum = 5; ringNum < 8; ringNum++ ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum - 3, lineColor);
        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      for ( int ringNum = 5; ringNum < 8; ringNum++ ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      MultiMatrix.fillScreen(bgColor);
      MultiMatrix.display();
    }
    while ( --iterations > 0 );
  }
}

//ringNum 0 = outer ring; ringNum 8 = innermost ring
void fillRingNum(int ringNum, char color) {

  for ( int pixel = ringNum; pixel < 16 - ringNum; pixel++ ) {
    MultiMatrix.fillPixel(ringNum, pixel, color);
    MultiMatrix.fillPixel(pixel, ringNum, color);    
    MultiMatrix.fillPixel(16 - ringNum - 1, pixel, color);
    MultiMatrix.fillPixel(pixel, 16 - ringNum - 1, color);
  }

}

void output(String input) {
  
   Serial.print(input+";"); 
}

// this handy function will return the number of bytes currently free in RAM, great for debugging!   
int freeRam(void)
{
  extern int  __bss_end; 
  extern int  *__brkval; 
  int free_memory; 
  if((int)__brkval == 0) {
    free_memory = ((int)&free_memory) - ((int)&__bss_end); 
  }
  else {
    free_memory = ((int)&free_memory) - ((int)__brkval); 
  }
  return free_memory; 
}
